This present invention is generally subject to the field of instrument and measurement technology based on Micro Electro-mechanical Systems (MEMS), especially relates to a novel design of micro navigation system.
Navigation is actually to determine position and attitude for a vehicle by using kinds of sensors. The conventional navigation schemes can be cataloged as: (1) Inertial navigation system, uses gyroscopes and accelerometers to measure rate of rotation and acceleration respectively. However gyroscope is always expensive, has relative big size, slow start-up and long-term drift. (2) Imaging navigation system is composed of camera, units for processing and recognizing, etc. But the imaging processing is complicated and difficult to be implemented in real time. (3) Global position system (GPS), comprises 24 satellites, receiver and signal-processor. It highly depends on the outside information from satellites.
Compared with the conventional methods, the burgeoning MEMS navigation equipments on the basis of micro silicon-based sensors can put miniaturization, practicability and intelligence together. The MEMS-based navigation systems take advantages of low cost, small size, lightweight and adequate performances with respect to the conventional systems.
MEMS is based on integration of micro-electron technology and micro-mechanics technology. A main representative of MEMS-based attitude system is Honeywell Digital Compass HMR3000. It is a tilt compensated compass that uses tilt sensor for enhanced performance up to xc2x140xc2x0 tilt range. Its composing and block diagram is shown in FIG. 1: liquid filled tilt sensors 11 determine inclination angles, the heading (or azimuth) is calculated 13 from the tilt information and the three magnetic field components detected by triaxial magnetometers 12, the angles then can be communicated with PC 15 through RS-232 14. HMR3000 measures 0xc2x0xcx9c360xc2x0 heading within xc2x140xc2x0 tilt, and the heading accuracy is xc2x11xc2x0 for level, but xc2x12xc2x0 for incline. The tilt range is only xc2x140xc2x0, and the tilt accuracy is xc2x10.4xc2x0 for 0xc2x0xcx9c20xc2x0, and xc2x10.6xc2x0 when exceed xc2x120xc2x0.
The disadvantages of HMR3000 are:
(1) The tilt range is small, only covers xc2x140xc2x0.
(2) The measurement accuracy will decline with bevel rising.
The present invention has a novel structure and attitude calculation algorithm to avoid the foregoing disadvantages.
The object of the present invention is to design a novel MEMS attitude system with nonrestrictive tilt range and excellent performance. The inventer designs architecture, composing of three-axis orthogonal accelerometers and three-axis orthogonal magnetometers, and schemes out the optimum attitude calculation algorithm for getting higher accuracy and higher reliability.
During sensors assembly, the directions of the tilt sensor (accelerometers) must be carefully aligned with the corresponding magnetic axes, and the three axes in the same orthogonal placement must be mutually orthogonalized strictly. It is critical for determining the measurement accuracy. Aiming at this assembling problem, the present invention constructs an optical alignment approach against the misalignment.
The present integrative system can attain overall attitude (xe2x88x9290xc2x0xcx9c90xc2x0 for pitch, xe2x88x92180xc2x0xcx9c180xc2x0 for roll, 0xc2x0xcx9c360xc2x0 for heading) with high accuracy in any tilt, and possesses characteristics as solid-state structure, high accuracy, small size, lightweight, low consumption, fast start-up and low cost.
The invention is about a novel micro azimuth-level detector based on MEMS, which comprises multi-sensors, A/D converter for converting the analog signals of the sensors to the digital signals, microprocessor for calculation of the digital signals, RS-232 for connecting the microprocessor with PC, and further comprises signal-processing in the microprocessor and Operation-Display software in PC, wherein multi-sensors are triaxial silicon accelerometers with the processing circuit and triaxial silicon magnetometers with the processing circuit.
The foregoing the signal-processing and the Operation-Display software include signal-syncretizing, attitude computation and graphical Operation-Display interface.
For overcoming the misalignment in assembling, a micro-cube with three micro-mirrors in orthogonal directions using for optical alignment is added to the system structure.
The signal-syncretizing is combination of signal-filter and electric/physical (E/P) signal conversion.
For signal-filter, 2nd-order FIR filter is employed:
y(n)=h0x(n)+h1x(nxe2x88x921)
where x(n),x(nxe2x88x921) are the output of sensor (input of the filter) and its backward shift, h0, h1 are constant coefficients, y(n) is the output of the filter.
For E/P signal-conversion, that is actually conversion from electrical voltage v(n) to corresponding physical signal u(n):
u(n)=(v(n)xe2x88x92v0)/kv
where v0 is zero-point, and kv is scale factor. v0 and kv are obtained in advance by calibration.
The attitude computation is as following:
Establish two reference coordinate systems: the geographic coordinate system (GCS) shown in FIG. 2(a) and the vehicle coordinate system (VCS) shown in FIG. 2(b). Defining the three orthogonal axes of the GCS as North21-East22-Ground23 (N-E-G), and the three orthogonal axes of the VCS as X-Y-Z (X 31 refers to the forward direction of the vehicle, Y 32 refers to the left-right direction of the vehicle, and Z 33 refers to downward direction of the vehicle).
The rotations of the VCS relative to the GCS represent the attitude status of the vehicle. The terms ROLL (xcex3), PITCH (xcex8) and HEADING ("psgr") are commonly used in aviation to represent attitude: ROLL refers to the rotation round the X-axis, PITCH refers to the rotation around the Y-axis, and HEADING refers to the rotation round the G-axis. The two representations of the same vector in GCS and VCS can be transformed each other through the Orientation Cosine Matrix Cnb.                               C          n          b                =                  [                                                                      cos                  ⁡                                      (                                          N                      ,                      X                                        )                                                                                                cos                  ⁡                                      (                                          E                      ,                      X                                        )                                                                                                cos                  ⁡                                      (                                          G                      ,                      X                                        )                                                                                                                        cos                  ⁡                                      (                                          N                      ,                      Y                                        )                                                                                                cos                  ⁡                                      (                                          E                      ,                      Y                                        )                                                                                                cos                  ⁡                                      (                                          G                      ,                      Y                                        )                                                                                                                        cos                  ⁡                                      (                                          N                      ,                      Z                                        )                                                                                                cos                  ⁡                                      (                                          E                      ,                      Z                                        )                                                                                                cos                  ⁡                                      (                                          G                      ,                      Z                                        )                                                                                ]                                        =                              [                          T              ij                        ]                                3            xc3x97            3                              
The element Tij of Cnb is composed of pitch, roll and heading. The three magnetometers are assembled along the X-Y-Z axes to form an orthogonal placement. And the assembling placement is also for the three accelerometers. The independent X, Y, and Z magnetic and gravity readings [xM,yM,zM]T and [xg,yg,zg]T, which figure the independent X, Y, Z components of the earth""s magnetic and gravity field, can be transformed back to the geographic coordinates by applying the Cosine Matrix Cnb. Further considering about errors [eMx eMy eMz]T and [egx egy egz]T, the following equations are attained (where H is magnetic hall effect, xcex2 is magnetic dip angle, fg is gravity acceleration):       [                                        x            M                                                            y            M                                                            z            M                                ]    =                              HC          n          b                ⁡                  [                                                                      cos                  ⁢                                      xe2x80x83                                    ⁢                  β                                                                                    0                                                                                      sin                  ⁢                                      xe2x80x83                                    ⁢                  β                                                              ]                    +              [                                                            e                Mx                                                                                        e                My                                                                                        e                Mz                                                    ]              ⇒          {                                                                                                                              H                      xc3x97                                              (                                                                                                            T                              11                                                        ⁢                            cos                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                    +                                                                                    T                              13                                                        ⁢                            sin                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                                          )                                                              +                                          e                      Mx                                                        =                                      x                    M                                                                                                                                                                  H                      xc3x97                                              (                                                                                                            T                              21                                                        ⁢                            cos                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                    +                                                                                    T                              23                                                        ⁢                            sin                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                                          )                                                              +                                          e                      My                                                        =                                      y                    M                                                                                                                                                                  H                      xc3x97                                              (                                                                                                            T                              31                                                        ⁢                            cos                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                    +                                                                                    T                              33                                                        ⁢                            sin                            ⁢                                                          xe2x80x83                                                        ⁢                            β                                                                          )                                                              +                                          e                      Mz                                                        =                                      z                    M                                                                                ⁢                      
                    [                                                                      x                  g                                                                                                      y                  g                                                                                                      z                  g                                                              ]                =                                                            C                n                b                            ⁡                              [                                                                            0                                                                                                  0                                                                                                                          f                        g                                                                                            ]                                      +                          [                                                                                          e                      gx                                                                                                                                  e                      gy                                                                                                                                  e                      gz                                                                                  ]                                ⇒                      {                                                                                                      x                      g                                        =                                                                                            T                          13                                                ⁢                                                  f                          g                                                                    +                                              e                        gx                                                                                                                                                                                    y                      g                                        =                                                                                            T                          23                                                ⁢                                                  f                          g                                                                    +                                              e                        gy                                                                                                                                                                                    z                      g                                        =                                                                                            T                          33                                                ⁢                                                  f                          g                                                                    +                                              e                        gz                                                                                                                                    
In the equations above, the errors can be eliminated through signal filter and error compensation.
From the foregoing equations, the three attitude angles are figured out via inverse tangent. The procedures of computation are:
Step one: calculating roll xcex3 according to Table 1 and value of xcex3ref
xcex3ref=arctg(yg/zg),
Step two: calculating pitch xcex8 according to
xcex8=xe2x88x92arctg(xgxc2x7cosxcex3/zg),
Step three: and calculating heading "psgr" according to Table 2, "psgr"ref, Xh and Yh,       Xh    =                            x          m                ⁢        cos        ⁢                  xe2x80x83                ⁢        θ            +                        (                                                    y                m                            ⁢              sin              ⁢                              xe2x80x83                            ⁢              γ                        +                                          z                m                            ⁢              cos              ⁢                              xe2x80x83                            ⁢              γ                                )                ⁢        sin        ⁢                  xe2x80x83                ⁢        θ                  Yh    =                            z          m                ⁢        sin        ⁢                  xe2x80x83                ⁢        γ            -                        y          m                ⁢        cos        ⁢                  xe2x80x83                ⁢        γ                                ψ        ref            =                        arc          ⁢                      xe2x80x83                    ⁢                      tg            ⁢                          (                                                                                          z                      m                                        ⁢                    sin                    ⁢                                          xe2x80x83                                        ⁢                    γ                                    -                                                            y                      m                                        ⁢                    cos                    ⁢                                          xe2x80x83                                        ⁢                    γ                                                                                                              x                      m                                        ⁢                    cos                    ⁢                                          xe2x80x83                                        ⁢                    θ                                    +                                                            (                                                                                                    y                            m                                                    ⁢                          sin                          ⁢                                                      xe2x80x83                                                    ⁢                          γ                                                +                                                                              z                            m                                                    ⁢                          cos                          ⁢                                                      xe2x80x83                                                    ⁢                          γ                                                                    )                                        ⁢                    sin                    ⁢                                          xe2x80x83                                        ⁢                    θ                                                              )                                      =                  arc          ⁢                      xe2x80x83                    ⁢                      tg            ⁢                          (                              Yh                Xh                            )                                            ,  
For eliminating the misalignment in assembling, a micro-cube is fixed on the circuit board. The micro-cube is a small right angle cube with three orthogonal coated mirrors, which are used for reflecting beams. It can be bought from Industrial Optics Company. It is a kind of externally coated cubic prism, which has an enhanced aluminum coating on the three orthogonal side-faces. The three beams upright the three orthogonal mirrored surfaces form a 3-D orthogonal beam adjustment X-Y-Z. X-Y-Z is defined as the datum frame of the groupware. The groupware is actually circuit board where three-axis accelerometers and three-axis magnetometers are installed. Define Xxe2x80x2-Yxe2x80x2-Zxe2x80x2 as the frame of the accelerometers, and Xxe2x80x3-Yxe2x80x3-Zxe2x80x3 as the frame of the magnetometers. The optical alignment is carried out through three steps below:
Step one: Three optical alignments
(a) Align X-Y-Z to North (magnetic north)-East-Ground by beam-based alignment. The independent components of the gravity field along N(X)-E(Y)-G(Z) axis are [0,0,fg]T, the independent components of the earth""s magnetic field along N(X)-E(Y)-G(Z) axis are [HN,0,HG]T. The relation between the three accelerometers"" readings [Xxe2x80x2a,Yxe2x80x2a,Zxe2x80x2a]T and [0,0,fg]T is:   {                                          X            a            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      X                    xe2x80x2                                    ,                  Z                                )                                                                                              Y            a            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      Y                    xe2x80x2                                    ,                  Z                                )                                                                                                            Z              a              xe2x80x2                        =                                          f                g                            ·                              cos                ⁡                                  (                                                            Z                      xe2x80x2                                        ,                    Z                                    )                                                              ,                      "AutoRightMatch"
The relation between the three magnetometers"" readings [Xxe2x80x3a,Yxe2x80x3a,Zxe2x80x3a]T and [HN,0,HG]T is:   {                                          X            a            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    X                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    Z                                    )                                                                                                              Y            a            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    X                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    Z                                    )                                                                                                                                            Z                a                xe2x80x3                            =                                                                    H                    N                                    ·                                      cos                    ⁡                                          (                                                                        Z                          xe2x80x3                                                ,                        X                                            )                                                                      +                                                      H                    G                                    ·                                      cos                    ⁡                                          (                                                                        Z                          xe2x80x3                                                ,                        Z                                            )                                                                                            ;                    ⁢                      xe2x80x83                                "AutoRightMatch"
(b) Align X-Y-Z to East-Ground-North(magnetic north) by beam based alignment. Here the relation between the three accelerometers"" readings [Xxe2x80x2b,Yxe2x80x2b,Zxe2x80x2b]T and the gravity XYZ-stage components [0,fg,0]T is:   {                                          X            b            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      X                    xe2x80x2                                    ,                  Y                                )                                                                                              Y            b            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      Y                    xe2x80x2                                    ,                  Y                                )                                                                                                            Z              b              xe2x80x2                        =                                          f                g                            ·                              cos                ⁡                                  (                                                            Z                      xe2x80x2                                        ,                    Y                                    )                                                              ,                      "AutoRightMatch"
The relation between the three magnetometers"" readings [Xxe2x80x3b,Yxe2x80x3b,Zxe2x80x3b]T and the magnetic XYZ-stage components [0,HG,HN]T is:   {                                          X            b            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    Z                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    Y                                    )                                                                                                              Y            b            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    Z                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    Y                                    )                                                                                                                                            Z                b                xe2x80x3                            =                                                                    H                    N                                    ·                                      cos                    ⁡                                          (                                                                        Z                          xe2x80x3                                                ,                        Z                                            )                                                                      +                                                      H                    G                                    ·                                      cos                    ⁡                                          (                                                                        Z                          xe2x80x3                                                ,                        Y                                            )                                                                                            ;                    ⁢                      xe2x80x83                                "AutoRightMatch"
(c) Align X-Y-Z to Ground-North(magnetic north)-East by beam based alignment. The relation between the three accelerometers"" readings [Xxe2x80x2c,Yxe2x80x2c,Zxe2x80x2c]T and the gravity XYZ-stage components [fg,0,0]T is:   {                                          X            c            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      X                    xe2x80x2                                    ,                  X                                )                                                                                              Y            c            xe2x80x2                    =                                    f              g                        ·                          cos              ⁡                              (                                                      Y                    xe2x80x2                                    ,                  X                                )                                                                                                            Z              c              xe2x80x2                        =                                          f                g                            ·                              cos                ⁡                                  (                                                            Z                      xe2x80x2                                        ,                    X                                    )                                                              ,                      "AutoRightMatch"
The relation between the three magnetometers"" readings [Xxe2x80x3c,Yxe2x80x3c,Zxe2x80x3c]T and the magnetic XYZ-stage components [HG,HN,0]T is:   {                                          X            c            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    Y                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            X                      xe2x80x3                                        ,                    X                                    )                                                                                                              Y            c            xe2x80x3                    =                                                    H                N                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    Y                                    )                                                      +                                          H                G                            ·                              cos                ⁡                                  (                                                            Y                      xe2x80x3                                        ,                    X                                    )                                                                                                                            Z              c              xe2x80x3                        =                                                            H                  N                                ·                                  cos                  ⁡                                      (                                                                  Z                        xe2x80x3                                            ,                      Y                                        )                                                              +                                                H                  G                                ·                                                      cos                    ⁡                                          (                                                                        Z                          xe2x80x3                                                ,                        X                                            )                                                        ⁢                                      xe2x80x83                                    .                                                              ⁢                      xe2x80x83                                "AutoRightMatch"
Step two: Determining the misaligned angles
Using the relation equations established in step one, figure out the Orientation Cosine Matrix Mg between the datum frame X-Y-Z and the acceleration frame   Mg  =      [                                        cos            ⁡                          (                                                X                  xe2x80x2                                ,                X                            )                                                            cos            ⁡                          (                                                X                  xe2x80x2                                ,                Y                            )                                                            cos            ⁡                          (                                                X                  xe2x80x2                                ,                Z                            )                                                                        cos            ⁡                          (                                                Y                  xe2x80x2                                ,                X                            )                                                            cos            ⁡                          (                                                Y                  xe2x80x2                                ,                Y                            )                                                            cos            ⁡                          (                                                Y                  xe2x80x2                                ,                Z                            )                                                                        cos            ⁡                          (                                                Z                  xe2x80x2                                ,                X                            )                                                            cos            ⁡                          (                                                Z                  xe2x80x2                                ,                Y                            )                                                            cos            ⁡                          (                                                Z                  xe2x80x2                                ,                Z                            )                                            ]  
and the Orientation Cosine Matrix Mm between X-Y-Z and the magnetic frame Xxe2x80x3-Yxe2x80x3-Zxe2x80x3,   Mm  =            [                                                  cos              ⁡                              (                                                      X                    xe2x80x3                                    ,                  X                                )                                                                        cos              ⁡                              (                                                      X                    xe2x80x3                                    ,                  Y                                )                                                                        cos              ⁡                              (                                                      X                    xe2x80x3                                    ,                  Z                                )                                                                                        cos              ⁡                              (                                                      Y                    xe2x80x3                                    ,                  X                                )                                                                        cos              ⁡                              (                                                      Y                    xe2x80x3                                    ,                  Y                                )                                                                        cos              ⁡                              (                                                      Y                    xe2x80x3                                    ,                  Z                                )                                                                                        cos              ⁡                              (                                                      Z                    xe2x80x3                                    ,                  X                                )                                                                        cos              ⁡                              (                                                      Z                    xe2x80x3                                    ,                  Y                                )                                                                        cos              ⁡                              (                                                      Z                    xe2x80x3                                    ,                  Z                                )                                                        ]        .  
Step three: Compensating the misaligned angles
When using the azimuth-level detector to measure attitude, always switch the outputs of the sensors into the orthogonal tuning X-Y-Z using the determined Orientation Cosine Matrix Mg and Mm.             [                                                  x              g                                                                          y              g                                                                          z              g                                          ]        =                  Mg                  -          1                    ⁡              [                                                                              x                  xe2x80x2                                g                                                                                                          y                  xe2x80x2                                g                                                                                                          z                  xe2x80x2                                g                                                    ]              ,      
    ⁢            [                                                  x              M                                                                          y              M                                                                          z              M                                          ]        =                            Mm                      -            1                          ⁡                  [                                                                                          x                    xe2x80x2                                    M                                                                                                                          y                    xe2x80x2                                    M                                                                                                                          z                    xe2x80x2                                    M                                                              ]                    .      
Where, [xxe2x80x2g,yxe2x80x2g,zxe2x80x2g]T, [xxe2x80x2M,yxe2x80x2M,zxe2x80x2M]T are respectively readings of three-axis accelerometers and three-axis magnetometers. And [xg,yg,zg]T,[xM,yM,zM]T are respectively the orthogonal tuning XYZ-stage components of the corresponding sensors.
The characteristics and effects are:
The present invention has largely improved on the composing, and the attitude computation algorithm. Three-axis silicon accelerometers make up of the inclinometer as the accelerometer can provide three independent orthogonal components of the earth""s gravity acceleration. The accelerometer-based inclinometer acts on not only the tilt-compensation, but also measurement for any pitch and roll. Through the Orientation Cosine Matrix Cnb, the relation equations between GCS and VCS can be established, further export the three attitude angles (pitch, roll, heading). The micro attitude detector of the invention is an integration of multi-MEMS-sensors, such as MEMS accelerometers and MEMS magnetometers. By taking the benefits of MEMS, the system takes on solid-state structure, small size, lightweight, low consumption, fast start-up and low cost.
Through the said inverse tangent algorithm of the invention are obtained high attitude accuracy in the full-scale range and excellent robust for withstanding effect field change. By optical alignment, the misalignment of each sensor axis deflective from the datum frame X-Y-Z are detected and compensated so as to make the attitude accuracy further largely enhanced.
As will be realized, the invention is capable of other and different embodiments, and its several details are capable of modifications in various obvious respects, all without departing from the invention. Accordingly, the drawings and description thereof are to be regarded as illustrative in nature, and not as restrictive.